#pragma execution_character_set("utf-8")

#include "cvstereo.h"
#include <QSettings>
#include <QFile>
#include <QTimer>
CVStereo::CVStereo(QObject *parent) : QObject(parent)
{
    if(!QFile::exists("./CalibrateResult/cameraCalibrate.ini"))
    {
        cout<<"cameraCalibrate.ini is not existent, please do calibration"<<endl;
        Matrix_left = cv::Mat(3 , 3, CV_64FC1, Scalar(0));
        dist_left = cv::Mat(1 , 5, CV_64FC1, Scalar(0));
        Matrix_right = cv::Mat(3 , 3, CV_64FC1, Scalar(0));
        dist_right = cv::Mat(1 , 5, CV_64FC1, Scalar(0));
    }
}
/**
 * 检测棋盘格内角点
 * imgs:图像
 * points:保存所有图像的内角点坐标
 * boardSize:棋盘格内角点数目（行，列）
*/
void CVStereo::findCornersPoints(vector<Mat> imgs, vector<vector<Point2f>> &points, Size boardSize)
{
    for (int i = 0; i < imgs.size(); i++)
    {
        Mat img1 = imgs[i];
        Mat gray1;
        cvtColor(img1, gray1, COLOR_BGR2GRAY);
        vector<Point2f> img1_points;
        findChessboardCorners(gray1, boardSize, img1_points);  //!计算方格标定板角点
        find4QuadCornerSubpix(gray1, img1_points, Size(5, 5));  //!细化方格标定板角点坐标
        points.push_back(img1_points);
    }
}
/**
 * 保存相机内参矩阵和畸变系数至ini文件
*/
void CVStereo::writeCamCalibrate()
{
    QSettings inifile("./CalibrateResult/cameraCalibrate.ini", QSettings::IniFormat);
    inifile.beginGroup("leftCamera");
    inifile.setValue("fx", Matrix_left.at<double>(0,0));
    inifile.setValue("fy", Matrix_left.at<double>(1,1));
    inifile.setValue("u0", Matrix_left.at<double>(0,2));
    inifile.setValue("v0", Matrix_left.at<double>(1,2));
    inifile.setValue("distortion0", dist_left.at<double>(0,0));
    inifile.setValue("distortion1", dist_left.at<double>(0,1));
    inifile.setValue("distortion2", dist_left.at<double>(0,2));
    inifile.setValue("distortion3", dist_left.at<double>(0,3));
    inifile.setValue("distortion4", dist_left.at<double>(0,4));
    inifile.endGroup();
    inifile.beginGroup("rightCamera");
    inifile.setValue("fx", Matrix_right.at<double>(0,0));
    inifile.setValue("fy", Matrix_right.at<double>(1,1));
    inifile.setValue("u0", Matrix_right.at<double>(0,2));
    inifile.setValue("v0", Matrix_right.at<double>(1,2));
    inifile.setValue("distortion0", dist_right.at<double>(0,0));
    inifile.setValue("distortion1", dist_right.at<double>(0,1));
    inifile.setValue("distortion2", dist_right.at<double>(0,2));
    inifile.setValue("distortion3", dist_right.at<double>(0,3));
    inifile.setValue("distortion4", dist_right.at<double>(0,4));
    inifile.endGroup();
}
/**
 * 读取ini文件至相机内参矩阵和畸变系数
*/
void CVStereo::readCamCalibrate()
{
    cout<<__func__<<endl;
    QSettings inifile("./CalibrateResult/cameraCalibrate.ini", QSettings::IniFormat);
    inifile.beginGroup("leftCamera");
    Matrix_left.at<double>(0,0) = inifile.value("fx").toDouble();
    Matrix_left.at<double>(1,1) = inifile.value("fy").toDouble();
    Matrix_left.at<double>(0,2) = inifile.value("u0").toDouble();
    Matrix_left.at<double>(1,2) = inifile.value("v0").toDouble();
    dist_left.at<double>(0,0) = inifile.value("distortion0").toDouble();
    dist_left.at<double>(0,1) = inifile.value("distortion1").toDouble();
    dist_left.at<double>(0,2) = inifile.value("distortion2").toDouble();
    dist_left.at<double>(0,3) = inifile.value("distortion3").toDouble();
    dist_left.at<double>(0,4) = inifile.value("distortion4").toDouble();
    inifile.endGroup();
    cout<<"Matrix_left"<<endl<<Matrix_left<<endl;
    cout<<"dist_left"<<endl<<dist_left<<endl;
    inifile.beginGroup("rightCamera");
    Matrix_right.at<double>(0,0) = inifile.value("fx").toDouble();
    Matrix_right.at<double>(1,1) = inifile.value("fy").toDouble();
    Matrix_right.at<double>(0,2) = inifile.value("u0").toDouble();
    Matrix_right.at<double>(1,2) = inifile.value("v0").toDouble();
    dist_right.at<double>(0,0) = inifile.value("distortion0").toDouble();
    dist_right.at<double>(0,1) = inifile.value("distortion1").toDouble();
    dist_right.at<double>(0,2) = inifile.value("distortion2").toDouble();
    dist_right.at<double>(0,3) = inifile.value("distortion3").toDouble();
    dist_right.at<double>(0,4) = inifile.value("distortion4").toDouble();
    inifile.endGroup();
    cout<<"Matrix_right"<<endl<<Matrix_right<<endl;
    cout<<"dist_right"<<endl<<dist_right<<endl;
}
/**
 * 保存双目立体标定结果至ini文件
*/
void writeStereoCalibrate()
{

}
/**
 * 读取ini文件至双目立体标定结果
*/
void readStereoCalibrate()
{

}
/**
 * 双目标定计算，保存标定结果
 * imgs_left:左眼图像
 * imgs_right：右眼图像
 * boardSize:方格标定板内角点数目（行，列）
*/
void CVStereo::stereoCalibCalculate(vector<Mat> imgs_left, vector<Mat> imgs_right, Size boardSize)
{
    findCornersPoints(imgs_left, imgLsPoints, boardSize);
    findCornersPoints(imgs_right, imgRsPoints, boardSize);

    Size squareSize = Size(10, 10);  //!棋盘格每个方格的真实尺寸，单位mm
    vector<vector<Point3f>> objectPoints;//!生成棋盘格每个内角点的空间三维坐标，z=0
    for (int i = 0; i < imgLsPoints.size(); i++)
    {
        vector<Point3f> tempPointSet;
        for (int j = 0; j < boardSize.height; j++)
        {
            for (int k = 0; k < boardSize.width; k++)
            {
                Point3f realPoint;
                //!假设标定板为世界坐标系的z平面，即z=0
                realPoint.x = j*squareSize.width;
                realPoint.y = k*squareSize.height;
                realPoint.z = 0;
                tempPointSet.push_back(realPoint);
            }
        }
        objectPoints.push_back(tempPointSet);
    }

    imageSize.width = imgs_left[0].cols;
    imageSize.height = imgs_right[0].rows;
    //!左右相机标定
    calibrateCamera(objectPoints, imgLsPoints, imageSize, Matrix_left, dist_left, rvecs, tvecs, 0);
    calibrateCamera(objectPoints, imgRsPoints, imageSize, Matrix_right, dist_right, rvecs, tvecs, 0);
    writeCamCalibrate();
    //!双目立体标定
    stereoCalibrate(objectPoints, imgLsPoints, imgRsPoints, Matrix_left, dist_left, Matrix_right, dist_right, imageSize, R, T, E, F, CALIB_USE_INTRINSIC_GUESS);

    //!保存标定参数
    cout << "R:" << endl << R << endl;
    cout << "T:" << endl << T << endl;
}
/**
 * 双目图像校正
 * imgs_left:左眼图像
 * imgs_right：右眼图像
*/
void CVStereo::stereoRectifyCalculate(vector<Mat> imgs_left, vector<Mat> imgs_right)
{
    //!计算校正变换矩阵
    stereoRectify(Matrix_left, dist_left, Matrix_right, dist_right, imageSize, R, T, R1, R2, P1, P2, Q, 0, -1, Size(), &validROIL, &validROIR);

    //!计算校正映射矩阵
    initUndistortRectifyMap(Matrix_left, dist_left, R1, P1, imageSize, CV_16SC2, map_lx, map_ly);
    initUndistortRectifyMap(Matrix_right, dist_right, R2, P2, imageSize, CV_16SC2, map_rx, map_ry);

    namedWindow("Rectify image");
    for (int i = 0; i < imgs_left.size(); i++)
    {
        //!进行校正映射
        remap(imgs_left[i], left_rectify, map_lx, map_ly, INTER_LINEAR);
        remap(imgs_right[i], right_rectify, map_rx, map_ry, INTER_LINEAR);

        //!拼接图像
        Mat result;
        hconcat(left_rectify, right_rectify, result);
        //!绘制直线，用于比较同一个内角点y轴是否一致
        line(result, Point(-1, imgLsPoints[i][0].y), Point(result.cols, imgLsPoints[i][0].y), Scalar(255, 0, 255), 1);
        imshow("Rectify image", result);
        waitKey(1000);
    }
}

/**
 * BM算法测距
*/
void CVStereo::stereo_BM_match(Mat& xyz)
{
    Ptr<StereoBM> bm = StereoBM::create(16, 9);
    bm->setBlockSize(2*blockSize+5);     //SAD窗口大小，5~21之间为宜
    bm->setROI1(validROIL);
    bm->setROI2(validROIR);
    bm->setPreFilterCap(31);
    bm->setMinDisparity(0);  //最小视差，默认值为0, 可以是负值，int型
    bm->setNumDisparities(numDisparities*16+16);//视差窗口，即最大视差值与最小视差值之差,窗口大小必须是16的整数倍，int型
    bm->setTextureThreshold(10);
    bm->setUniquenessRatio(uniquenessRatio);//uniquenessRatio主要可以防止误匹配
    bm->setSpeckleWindowSize(100);
    bm->setSpeckleRange(32);
    bm->setDisp12MaxDiff(-1);
    Mat disp, disp8;
    if(left_rectify.channels() != 1)
    {
        cvtColor(left_rectify, left_rectify, COLOR_BGR2GRAY);
        cout<<"cvtColor left_rectify"<<endl;
    }
    if(right_rectify.channels() != 1)
    {
        cvtColor(right_rectify, right_rectify, COLOR_BGR2GRAY);
        cout<<"cvtColor right_rectify"<<endl;
    }
    bm->compute(left_rectify, right_rectify, disp);//输入图像必须为灰度图
    disp.convertTo(disp8, CV_8U, 255 / ((numDisparities * 16 + 16)*16.));//计算出的视差是CV_16S格式
    reprojectImageTo3D(disp, xyz, Q, true); //在实际求距离时，ReprojectTo3D出来的X / W, Y / W, Z / W都要乘以16(也就是W除以16)，才能得到正确的三维坐标信息。
    xyz = xyz * 16;
    imshow("disparity", disp8);
}

/**
 * SGBM算法测距
*/
void CVStereo::stereo_SGBM_match(Mat& xyz)
{
    int mindisparity = 0;
    int ndisparities = 32;
    int SADWindowSize = 3;
    cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create(mindisparity, ndisparities, SADWindowSize);
//    cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create(3, 16, 3);

    int P1 = 8 * left_rectify.channels() * SADWindowSize* SADWindowSize;
    int P2 = 32 * left_rectify.channels() * SADWindowSize* SADWindowSize;
    sgbm->setP1(P1);
    sgbm->setP2(P2);
    sgbm->setPreFilterCap(15);
    sgbm->setUniquenessRatio(6);
    sgbm->setSpeckleRange(2);
    sgbm->setSpeckleWindowSize(100);
    sgbm->setDisp12MaxDiff(1);
        //sgbm->setNumDisparities(1);
    sgbm->setMode(cv::StereoSGBM::MODE_HH);
    Mat disp,disp8U;
    if(left_rectify.channels() != 1)
    {
        cvtColor(left_rectify, left_rectify, COLOR_BGR2GRAY);
        cout<<"cvtColor left_rectify"<<endl;
    }
    if(right_rectify.channels() != 1)
    {
        cvtColor(right_rectify, right_rectify, COLOR_BGR2GRAY);
        cout<<"cvtColor right_rectify"<<endl;
    }

    sgbm->compute(left_rectify, right_rectify, disp);
    disp.convertTo(disp, CV_32F, 1.0 / 16);                //除以16得到真实视差值
    disp8U = Mat(disp.rows, disp.cols, CV_8UC1);       //显示
    //normalize(disp, disp8U, 0, 255, NORM_MINMAX, CV_8UC1);
    disp.convertTo(disp8U,CV_8U,255/(numDisparities*16.));
    reprojectImageTo3D(disp, xyz, Q);
    xyz = xyz * 16;
    cout<<disp8U.size()<<endl;
    imshow("disparity",disp8U);//视差图
}

Point2f CVStereo::findObjectLeft(cv::Mat& src)
{
    cv::Mat dst;
    undistort(src, dst, Matrix_left, dist_left);//!畸变校正

    return Point2f(0, 0);
}
Point2f CVStereo::findObjectRight(cv::Mat& src)
{
    cv::Mat dst;
    undistort(src, dst, Matrix_right, dist_right);//!畸变校正
    return Point2f(0, 0);
}
Point3f CVStereo::uv2xyz(Point2f uvLeft,Point2f uvRight)
{
    //  [u1]      |X|					  [u2]      |X|
    //Z*|v1| = Ml*|Y|					Z*|v2| = Mr*|Y|
    //  [ 1]      |Z|					  [ 1]      |Z|
    //			  |1|								|1|
    Mat mLeftRotation = Mat(3,3,CV_32F,leftRotation);
    Mat mLeftTranslation = Mat(3,1,CV_32F,leftTranslation);
    Mat mLeftRT = Mat(3,4,CV_32F);//左相机M矩阵，M=[R T]
    hconcat(mLeftRotation,mLeftTranslation,mLeftRT);
//    Mat mLeftIntrinsic = Mat(3,3,CV_32F,leftIntrinsic);
    Mat mLeftIntrinsic = Matrix_left;//!左相机内参矩阵
    Mat mLeftM = mLeftIntrinsic * mLeftRT;
    //cout<<"左相机M矩阵 = "<<endl<<mLeftM<<endl;

//    Mat mRightRotation = Mat(3,3,CV_32F,rightRotation);
    Mat mRightRotation = R;//!由stereoCalibrate得到，右相机到左相机的旋转矩阵
//    Mat mRightTranslation = Mat(3,1,CV_32F,rightTranslation);
    Mat mRightTranslation = T;//!由stereoCalibrate得到，右相机到左相机的平移矩阵
    Mat mRightRT = Mat(3,4,CV_32F);//右相机M矩阵
    hconcat(mRightRotation,mRightTranslation,mRightRT);
//    Mat mRightIntrinsic = Mat(3,3,CV_32F,rightIntrinsic);
    Mat mRightIntrinsic = Matrix_right;//!左相机内参矩阵
    Mat mRightM = mRightIntrinsic * mRightRT;
    //cout<<"右相机M矩阵 = "<<endl<<mRightM<<endl;

    //最小二乘法A矩阵
    Mat A = Mat(4,3,CV_32F);
    A.at<float>(0,0) = uvLeft.x * mLeftM.at<float>(2,0) - mLeftM.at<float>(0,0);
    A.at<float>(0,1) = uvLeft.x * mLeftM.at<float>(2,1) - mLeftM.at<float>(0,1);
    A.at<float>(0,2) = uvLeft.x * mLeftM.at<float>(2,2) - mLeftM.at<float>(0,2);

    A.at<float>(1,0) = uvLeft.y * mLeftM.at<float>(2,0) - mLeftM.at<float>(1,0);
    A.at<float>(1,1) = uvLeft.y * mLeftM.at<float>(2,1) - mLeftM.at<float>(1,1);
    A.at<float>(1,2) = uvLeft.y * mLeftM.at<float>(2,2) - mLeftM.at<float>(1,2);

    A.at<float>(2,0) = uvRight.x * mRightM.at<float>(2,0) - mRightM.at<float>(0,0);
    A.at<float>(2,1) = uvRight.x * mRightM.at<float>(2,1) - mRightM.at<float>(0,1);
    A.at<float>(2,2) = uvRight.x * mRightM.at<float>(2,2) - mRightM.at<float>(0,2);

    A.at<float>(3,0) = uvRight.y * mRightM.at<float>(2,0) - mRightM.at<float>(1,0);
    A.at<float>(3,1) = uvRight.y * mRightM.at<float>(2,1) - mRightM.at<float>(1,1);
    A.at<float>(3,2) = uvRight.y * mRightM.at<float>(2,2) - mRightM.at<float>(1,2);

    //最小二乘法B矩阵
    Mat B = Mat(4,1,CV_32F);
    B.at<float>(0,0) = mLeftM.at<float>(0,3) - uvLeft.x * mLeftM.at<float>(2,3);
    B.at<float>(1,0) = mLeftM.at<float>(1,3) - uvLeft.y * mLeftM.at<float>(2,3);
    B.at<float>(2,0) = mRightM.at<float>(0,3) - uvRight.x * mRightM.at<float>(2,3);
    B.at<float>(3,0) = mRightM.at<float>(1,3) - uvRight.y * mRightM.at<float>(2,3);

    Mat XYZ = Mat(3,1,CV_32F);
    //采用SVD最小二乘法求解XYZ
    solve(A,B,XYZ,DECOMP_SVD);

    //cout<<"空间坐标为 = "<<endl<<XYZ<<endl;

    //世界坐标系中的坐标，实际为左相机坐标系下的坐标
    Point3f world;
    world.x = XYZ.at<float>(0,0);
    world.y = XYZ.at<float>(1,0);
    world.z = XYZ.at<float>(2,0);

    return world;
}

cv::Mat CVStereo::rectify_undistort(cv::Mat& src)
{
    cv::Mat dst;
    undistort(src, dst, Matrix_left, dist_left);//!畸变校正
    line(dst, Point(0, 100), Point(dst.cols, 100), Scalar(255, 0, 255), 1);
    return dst;
}
cv::Mat CVStereo::rectify_remap(cv::Mat& src)
{
    cv::Mat dst;
    //计算映射坐标矩阵
    Mat R = Mat::eye(3, 3, CV_32F);
    Mat mapx = Mat(imageSize, CV_32FC1);
    Mat mapy = Mat(imageSize, CV_32FC1);
    initUndistortRectifyMap(Matrix_left, dist_left, R, Matrix_left, imageSize, CV_32FC1, mapx, mapy);
    remap(src, dst, mapx, mapy, INTER_LINEAR);
    line(dst, Point(0, 100), Point(dst.cols, 100), Scalar(255, 0, 255), 1);
    return dst;
}
